module dig_dp(clk,rst_n,Xmeas,cfg_data,eep_rd_data,src1sel,src0sel,
              cmplmnt,saturate,dst2P,dst2R,dst2Err,dst2PrevErr,
              dst2SumErr,dst2DutyWrk,dst2Xset,dst,BoothSel);


input clk;			// clock = 800MHz
input rst_n;			// active low reset (deassert on negedge clock)
input [13:0] Xmeas;		// Measured result from accelerometer 
input [13:0] cfg_data;		// lower 14-bits of data from UART
input [13:0] eep_rd_data;	// data from EEPROM

/////// Type defs for SRC bus selects //////////////
typedef enum logic [2:0] {PrevErr2Src0, SumErr2Src0, DutyWrk2Src0, EEP2Src0,
                          Xset2Src0, posAck2Src0, zero2Src0} src0sel_t;
typedef enum logic [2:0] {P2Src1, MultRes2Src1, Xmeas2Src1, CfgData2Src1,
                          Err2Src1, zero2Src1} src1sel_t;

input src0sel_t src0sel;
input src1sel_t src1sel;
input cmplmnt;			// Src0 data should be 2's complemented
input saturate;			// result of addition should be saturated
input dst2R;			// ALU result written to bits [12:1] of P_reg (prepare for multiply)
input dst2P;			// ALU result written to upper portion of P_reg with ASR
input dst2Err;			// ALU result written to ERR register
input dst2PrevErr;		// ALU result written to Previous Error register
input dst2SumErr;		// ALU result written to SumErr register
input dst2DutyWrk;		// ALU result written to DutyWrk accumulator register
input dst2Xset;			// ALU result written to Xset register (set point) 

output [13:0] dst;		// ALU result (goes to PWM, and to UART for transmission)
output [1:0] BoothSel;		// lower 2-bits of P_reg used for Booth selecting

///////////////////////////////////////////////////////////////
// The following are registers needed for math calculations //
/////////////////////////////////////////////////////////////
reg [28:0] P_reg;	// Multiply accumulate/shift register (product here)
reg [13:0] Err;		// stores Error term (SAR - SetPt)
reg [13:0] PrevErr;	// stores previous error term so derivative can be calc
reg [13:0] SumErr;	// running sum of error terms for integral
reg [13:0] Xset;	// Desired X acceleration set point 
reg [13:0] DutyWrk;	// Duty math working register

wire [13:0] Src1,Src0,preSrc0,sum;	// intermediate signals needed;
wire co;

/////////////////////////////////////////////////////////////
// Implement the P_reg (product accumalate register) next //
///////////////////////////////////////////////////////////
always @(posedge clk)
  if (dst2R)
    P_reg <= {14'h0000,dst,1'b0};		// initialize for upcoming multiply
  else if (dst2P)
    P_reg <= {dst[13],dst[13:0],P_reg[14:1]};	// upper bits load plus ASR during multiply

//////////////////////////////////
// Implement the Xset register //
////////////////////////////////
always @(posedge clk, negedge rst_n)
  if (!rst_n)
    Xset <= 14'b0000;
  else if (dst2Xset)
    Xset <= dst;

/////////////////////////////////
// Implement the Err register //
///////////////////////////////
always @(posedge clk)
  if (dst2Err)
    Err <= dst;

/////////////////////////////////////
// Implement the PrevErr register //
///////////////////////////////////
always @(posedge clk,negedge rst_n)
  if (!rst_n)
    PrevErr <= 14'h0000;	// PrevErr needs reset because it is used prior to ever being written
  else if (dst2PrevErr)
    PrevErr <= dst;

////////////////////////////////////
// Implement the SumErr register //
//////////////////////////////////
always @(posedge clk, negedge rst_n)
  if (!rst_n)
    SumErr <= 14'h0000;
  else if (dst2SumErr)
    SumErr <= dst;

/////////////////////////////////////
// Implement the DutyWrk register //
///////////////////////////////////
always @(posedge clk)
  if (dst2DutyWrk)
    DutyWrk <= dst;

/////////////////////////////////////
// Implement the Src1 muxing next //
///////////////////////////////////
assign Src1 = (src1sel == P2Src1) 	? P_reg[28:15] :
              (src1sel == MultRes2Src1) ? P_reg[25:12] :
              (src1sel == Xmeas2Src1) 	? Xmeas :
              (src1sel == CfgData2Src1)	? cfg_data :
              (src1sel == Err2Src1)	? Err :
              14'h0000;				// 0x0000 is the default choice

////////////////////////////////////////
// Implement the preSrc0 muxing next //
//////////////////////////////////////
assign preSrc0 = (src0sel == PrevErr2Src0)	? PrevErr : 
                 (src0sel == SumErr2Src0)	? SumErr :
                 (src0sel == DutyWrk2Src0) 	? DutyWrk :
                 (src0sel == EEP2Src0) 		? eep_rd_data :
		 (src0sel == Xset2Src0)         ? Xset :
                 (src0sel == posAck2Src0)	? 14'h0A5A :
                 14'h0000;			// 0x0000 is the default choice 

////////////////////////////////////////
// Implement 2's Complement for Src0 //
//////////////////////////////////////
assign Src0 = (cmplmnt) ? ~preSrc0 : preSrc0;
assign Cin = (cmplmnt) ? 1'b1 : 1'b0;

/////////////////////////////
// Implement the addition //
///////////////////////////
assign {co,sum}  = Src1 + Src0 + Cin;

///////////////////////////////
// Now for saturation logic //
/////////////////////////////
assign sat_pos = saturate & (~Src1[13] & ~Src0[13]) & sum[13] |		// both positive, yet result negative
                 (src1sel==MultRes2Src1) & ~P_reg[28]&(P_reg[27] | P_reg[26] | P_reg[25]);	// multiply sat + condition
assign sat_neg = saturate & (Src1[13] & Src0[13]) & ~sum[13] |		// both negative, yet result positive
                 (src1sel==MultRes2Src1) & P_reg[28]&(~P_reg[27] | ~P_reg[26] | ~P_reg[25]);	// multiply sat - condition

assign dst = (sat_pos) ? 14'h1FFF :
             (sat_neg) ? 14'h2000 :
             sum;
///////////////////////////////////////////////
// Control needs 2-LSB's of P_reg to        // 
// select: EEP,000,-EEP for mult algorithm //
////////////////////////////////////////////
assign BoothSel = P_reg[1:0];

endmodule
